function [fB, omegaIBB, accTriadJfCond, gyroTriadJfCond] = imuout2in_cont(fTildeOutS, omegaTildeIBOutS, IMUPar, omegaIBBRate, innerIterAlgoType, outerIterStepNum, innerIterStepNum) %#codegen
%IMUOUT2IN_CONT 将器件的输出值转换为输入值
%
% Tips
% # 本函数针对连续形式
%
% Input Arguments
% # fTildeOutS: 3*n矩阵，单位时间加速度计输出脉冲数
% # omegaTildeOutS: 3*n矩阵，单位时间陀螺输出脉冲数
% # IMUPar: 包含acc域（加速度计三元组）和gyro域（陀螺三元组）的确定性误差参数的结构体，参数详细说明参考accerror_cont及gyroerror_cont函数
% # omegaIBBRate: 3*n矩阵，omegaTildeOutS各时刻的角加速度，单位rad/s^2
% # innerIterAlgoType：int8标量，2*1向量，int8，加表，陀螺内部迭代使用的算法类型，默认加表为1，陀螺为2
%                innerIterAlgoType = 1，牛顿迭代
%                innerIterAlgoType = 其它值，线性迭代
% # outerIterStepNum: 标量，外部迭代步数，默认为2
% # innerIterStepNum: 标量，内部迭代步数，仅当使用牛顿迭代时有效，默认为2
%
% Output Arguments
% # fB: 3*n矩阵，经补偿后的器件输入比力值
% # omegaIBB: 3*n矩阵，经补偿后的器件输入角速度值
% # accTriadJfCond: m*1列向量，加速度计三元组Jf矩阵条件数，无单位
% # gyroTriadJfCond: m*1列向量，陀螺三元组Jf矩阵条件数，无单位
%
% References:
% # 《应用导航算法工程基础》“由输出值计算输入值的算法”

if nargin < 5
    innerIterAlgoType = [int8(1); int8(2)];
end
if nargin < 6
    outerIterStepNum = 2;
end
if nargin < 7
    innerIterStepNum = 2;
end

assert(isequal(size(innerIterAlgoType), size(NaN(2, 1))));
assert(isequal(size(fTildeOutS), size(omegaTildeIBOutS)));
assert(isequal(size(IMUPar.acc.KScal0, 2), size(IMUPar.acc.PS0B, 3), size(IMUPar.acc.dfBiasS, 2), size(IMUPar.acc.dKScalP, 3),...
    size(IMUPar.acc.dKScalN, 3), size(IMUPar.acc.dPSS0, 3), size(IMUPar.acc.dfQuantS, 2), size(IMUPar.acc.lB, 3), ...
    size(IMUPar.acc.KAniso, 2), size(IMUPar.acc.CBA, 4)));
assert(isequal(size(IMUPar.gyro.KScal0, 2), size(IMUPar.gyro.PS0B, 3), size(IMUPar.gyro.domegaIBBiasS, 2), size(IMUPar.gyro.dKScalP, 3),...
    size(IMUPar.gyro.dKScalN, 3), size(IMUPar.gyro.dPSS0, 3), size(IMUPar.gyro.domegaIBQuantS, 2), size(IMUPar.gyro.DGSens, 3)));

m = size(fTildeOutS, 2);
if m == size(IMUPar.acc.KScal0, 2)
    isIMUParFullSize = true;
else
    isIMUParFullSize = false;
end

fB = NaN(size(fTildeOutS));
omegaIBB = NaN(size(omegaTildeIBOutS));
if nargout >= 3
    accTriadJfCond = NaN(m, 1);
end
if nargout >= 4
    gyroTriadJfCond = NaN(m, 1);
end
for j=1:m
    %% 计算标度因数误差阵
    if isIMUParFullSize
        idx = j;
    else
        idx = 1;
    end
    accdKScalCoef = scalfacterrsign(IMUPar.acc.dKScalP(:, :, idx), IMUPar.acc.dKScalN(:, :, idx), fTildeOutS(:, j));
    gyrodKScalCoef = scalfacterrsign(IMUPar.gyro.dKScalP(:, :, idx), IMUPar.gyro.dKScalN(:, :, idx), omegaTildeIBOutS(:, j));
    
    %% 对输出值按标度因数标称值、零偏进行补偿得到迭代初值
    accPSBT = (IMUPar.acc.PS0B(:, :, idx)*(eye(3)+IMUPar.acc.dPSS0(:, :, idx)))';
    [fB0, accTriadJfCond(j, 1)] = sensorout2in_linear(fTildeOutS(:, j), 0, zeros(3, 1), accPSBT, ...
        IMUPar.acc.KScal0(:, idx), accdKScalCoef, IMUPar.acc.dfBiasS(:, idx), IMUPar.acc.dfQuantS(:, idx));
    if accTriadJfCond(j, 1) > 20
        warning(['Jf condition number of acc triad of sample ' int2str(j) ' reaches ' num2str(accTriadJfCond(j, 1))]);
    end
    gyroPSBT = (IMUPar.gyro.PS0B(:, :, idx)*(eye(3)+IMUPar.gyro.dPSS0(:, :, idx)))';
    [omegaIBB0, gyroTriadJfCond(j, 1)] = sensorout2in_linear(omegaTildeIBOutS(:, j), 0, zeros(3, 1), gyroPSBT, ...
        IMUPar.gyro.KScal0(:, idx), gyrodKScalCoef, IMUPar.gyro.domegaIBBiasS(:, idx), IMUPar.gyro.domegaIBQuantS(:, idx));
    if gyroTriadJfCond(j, 1) > 20
        warning(['Jf condition number of gyro triad of sample ' int2str(j) ' reaches ' num2str(gyroTriadJfCond(j, 1))]);
    end
    
    %% 内外层迭代
    fOuter = fB0;
    omegaOuter = omegaIBB0;
    for iOut =1:outerIterStepNum
        %% 计算外层迭代g(v, w, p)
        % 杆臂效应误差-比力形式
        dfSizeS = acclevarmerr_cont(accPSBT, IMUPar.acc.lB(:, :, idx), omegaOuter, omegaIBBRate(:, j));
        % 不等惯量误差-比力形式
        dfAnisoS = accanisoerr_cont(IMUPar.acc.KAniso(:, idx), IMUPar.acc.CBA(:, :, :, idx), omegaOuter);
        % g敏感项误差-角速度形式
        domegaIBGSensS = IMUPar.gyro.DGSens(:, :, idx)*fOuter;
        % 计算KScal NOTE: vTildeScalS计算式应与accerror、gyroerror及fgjacobian函数一致
        [~, fKScal] = polyscalfact(accPSBT*fOuter+IMUPar.acc.dfBiasS(:, idx)+dfSizeS+dfAnisoS, ...
            accdKScalCoef, IMUPar.acc.KScal0(:, idx));
        [~, omegaKScal] = polyscalfact(gyroPSBT*omegaOuter+IMUPar.gyro.domegaIBBiasS(:, idx)+domegaIBGSensS, ...
            gyrodKScalCoef, IMUPar.gyro.KScal0(:, idx));
        % 计算g(v, w, p)的值
        gfOuter = fKScal.*(dfSizeS+dfAnisoS);
        gomegaOuter = omegaKScal.*domegaIBGSensS;
        % 调用加表内层迭代的函数
        switch innerIterAlgoType(1)
            case int8(1)
                fOuter = sensorout2in_newton(fTildeOutS(:, j), gfOuter, fOuter, accPSBT, ...
                    IMUPar.acc.KScal0(:, idx), accdKScalCoef, IMUPar.acc.dfBiasS(:, idx), IMUPar.acc.dfQuantS(:, idx), dfSizeS+dfAnisoS, innerIterStepNum);
            otherwise
                fOuter = sensorout2in_linear(fTildeOutS(:, j), gfOuter, fOuter, accPSBT, ...
                    IMUPar.acc.KScal0(:, idx), accdKScalCoef, IMUPar.acc.dfBiasS(:, idx), IMUPar.acc.dfQuantS(:, idx));
        end
        % 调用陀螺内层迭代的函数
        switch innerIterAlgoType(2)
            case int8(1)
                omegaOuter = sensorout2in_newton(omegaTildeIBOutS(:, j), gomegaOuter, omegaOuter, gyroPSBT, ...
                    IMUPar.gyro.KScal0(:, idx), gyrodKScalCoef, IMUPar.gyro.domegaIBBiasS(:, idx), IMUPar.gyro.domegaIBQuantS(:, idx), domegaIBGSensS, innerIterStepNum);
            otherwise
                omegaOuter = sensorout2in_linear(omegaTildeIBOutS(:, j), gomegaOuter, omegaOuter, gyroPSBT, ...
                    IMUPar.gyro.KScal0(:, idx), gyrodKScalCoef, IMUPar.gyro.domegaIBBiasS(:, idx), IMUPar.gyro.domegaIBQuantS(:, idx));
        end
    end
    fB(:, j) = fOuter;
    omegaIBB(:, j) = omegaOuter;
end
end